#include "WPILib.h"
#ifndef GATE_CPP
#define GATE_CPP
#define GATE_OPEN_TIME .1 // seconds
#define GATE_HOLD_TIME .15
#define GATE_CLOSE_TIME .1
#define GATE_HOLD_CLOSE_TIME 1
#define DEFAULT_CHANNEL 9
#define GATE_SPEED -1

class Gate
{
public:
	Gate(unsigned char Channel = DEFAULT_CHANNEL)
	:Jag(Channel)	
	{
		GateState = 5; //closing the gate
		GateTime.Stop();//Stops and resets timer
		GateTime.Reset();
		EnableFlg = true;
		RunFlag = true;
	}
	void Run(void)
	{
		Idle();//Stating positions of gate
		GateOpenSet();
		GateOpen();
		GateHoldSet();
		GateHold();
		GateCloseSet();
		GateClose();
		GateHoldCloseSet();
		GateHoldClose();
	}
	void Start(void)
	{
		printf("Start\n");
		if(EnableFlg)
		{
			printf("Start-Enabled\n");

			if(GateState == 0)GateState = 1;//Starts gate process
		}
	}
	bool IsRunning(void)//checks if gate is already running
	{
		return RunFlag;
	}
	bool Enable(bool Flg)//Lets gate know if it's alright to run
	{
		EnableFlg = Flg;
		return EnableFlg;
	}
	bool Enable(void)
	{
		return EnableFlg;
	}
protected:
	Timer GateTime;
	unsigned int GateState;
	bool EnableFlg, RunFlag;
	CANJaguar Jag;
	
	
	void Idle(void)
	{
		if(GateState == 0)
		{
		Jag.Set(0);//Sets speed of jag
		}
	}
	void GateOpenSet(void)//Initialization of gate process
	{
		if(GateState == 1)
		{
			printf("GateOpenSet\n");

		GateTime.Reset();
		GateTime.Start();
		GateState = 2;
		}
	}
	void GateOpen(void)//Opening Gate
	{
		if(GateState == 2)
			{
			if(GateTime.Get()>= GATE_OPEN_TIME)//Checks time
				{
				printf("GateOpen\n");
					GateState = 3;//Switches GateStates	
				}
			Jag.Set(GATE_SPEED);//Sets jag and moves motor
			}
	}
	void GateHoldSet(void)//Initializes gate pause
	{
		if(GateState == 3)//Checks Gatestate
		{
			printf("GateHoldSet\n");

			GateTime.Reset();//Resets and starts timer
			GateTime.Start();
			GateState = 4;//Switches GateStates

		}
	}
	void GateHold(void)//Pauses gate
	{
		if(GateState == 4)
					{
			printf("GateHold\n");

					if(GateTime.Get()>= GATE_HOLD_TIME)
						{
							GateState = 5;
						}
					Jag.Set(0);
					}
	}
	void GateCloseSet(void)//initializes gate closing
	{
		if(GateState == 5)
		{
			printf("GateCloseSet\n");
			GateTime.Reset();
			GateTime.Start();
			GateState = 6;
			

		}
	}
	void GateClose(void)//closes gate
	{
		if(GateState == 6)
		{
			printf("GateClose\n");

					if(GateTime.Get()>= GATE_CLOSE_TIME)
						{
							GateState = 7;
						}
					Jag.Set(-1*GATE_SPEED);
					}
	}
	void GateHoldCloseSet(void)//initializes pause period
	{
		if(GateState == 7)
		{
			printf("GateHoldCloseSet\n");

			GateTime.Reset();
			GateTime.Start();
			GateState = 8;
		

		}
	}
	void GateHoldClose(void)//Pauses gate
	{
		if (GateState == 8)
		{
			printf("GateHoldClose\n");

			if(GateTime.Get()>= GATE_HOLD_CLOSE_TIME)
			{
				GateState = 0;	
			}
			Jag.Set(0);
		}
	}

};


#endif

